% clc,clear
close all
% load("SineInput");
Time = yout{1}.Values.Time;
ExpectTheta = yout{1}.Values.Data;
ExpectTd = yout{2}.Values.Data;
RealTheta = yout{3}.Values.Data;
figure(1)
plot(Time,ExpectTheta,Time,RealTheta,Time,ExpectTd);
legend("ExpectTheta","RealTheta","ExpectTd");
a1 = -0.03078;
b1 = 0.05317;
a2 = -0.02209;
b2 = 0.1104;
a3 = -0.00563;
b3 = 0.1704;
Tk1 = a1*sin(b1*(-RealTheta-15))+a2*sin(b2*(-RealTheta-15))+a3*sin(b3*(-RealTheta-15));
Tk2 = a1*sin(b1*(-RealTheta+15))+a2*sin(b2*(-RealTheta+15))+a3*sin(b3*(-RealTheta+15));
u1 = (ExpectTd.*Tk1)./(Tk1.^2+Tk2.^2);
u2 = (ExpectTd.*Tk2)./(Tk1.^2+Tk2.^2);
figure(2)
plot(Time,u1,Time,u2);
legend("u(1)","u(2)");




